#include "Measurement.h"


Measurement::Measurement():uni_dist(-1,1),
						   generator(time(NULL)),
						   uni(generator, uni_dist),
						   H(1,2)
{
	//Obervation Matrix, Observe on position
	H(0,0) = 1;
	H(0,1) = 0;

	meas_noise = 0;
}

Measurement::Measurement(boost::numeric::ublas::matrix<double> _state , double _meas_noise):uni_dist(-1,1),
																						    generator(time(NULL)),
																					        uni(generator, uni_dist),
																							H(1,2)
{
	//Obervation Matrix, Observe on position
	H(0,0) = 1;
	H(0,1) = 0;

	meas_noise = _meas_noise;
}

boost::numeric::ublas::matrix<double> Measurement::meas(boost::numeric::ublas::matrix<double> _state)
{

	boost::numeric::ublas::matrix<double> ret(1,1);

	ret = prod( H , _state );

	ret(0,0) = ret(0,0) + meas_noise * uni();

	return ret;

}